//
//  Ransac.c
//  Vector_Math
//
//  Created by John R Peterson on 3/13/12.
//  Copyright 2012 __MyCompanyName__. All rights reserved.
//

#include <stdio.h>


#include "Ransac.h"
#include <stdlib.h>

#define PTHRESHOLD 0.1 // perpendicular threshold to count as an inlier in ransac
#define CTHRESHOLD 0.2 // parallel threshold to count as an ilier
#define ITERATIONS 20 // how many should each ransac iteration do
#define MINLINEPOINTS 10 // how many points does a line need to have in it to count

LineList* Ransac_fitLines(VecList *vlist) {
    
    LineList *lines = LineList_create();
    
    int numpoints = 100000;
    Line *candidate;
    
    int i, j;
    while ((numpoints >= MINLINEPOINTS) && (vlist->size >= MINLINEPOINTS) && (vlist->size >= 2)) {
        
        candidate = Ransac_iterate(vlist);
        numpoints = candidate->points->size;
        
        if (numpoints >= MINLINEPOINTS) {
            // got a good line
            LineList_add(lines,candidate);
            
            // remove its inlier set from the vector list don't detroy the points
            // since the the lines hold them
            for (i = 0; i < candidate->points->size; i++) {
                Vec *target = candidate->points->list[i];
                for (j = 0; j < vlist->size; j++) {
                    if (Vec_equals(target,vlist->list[j])) {
                        break;
                    }
                }
                VecList_remove(vlist, j);
            }
        }
    }
    
    return lines;
}

// TODO a single itteration of ransac which returns the best line
Line* Ransac_iterate(VecList *vlist) {
    
    int cntr = 0;
    Line *curL, *bestL;
    VecList *curInliers, *bestInliers;
    
    while (cntr < ITERATIONS) {
        
        // grab a random line
        curL = Ransac_randomLine(vlist);
        
        // compute the inlier set
        curInliers = Ransac_inlierSet(vlist, curL);

        // must keep it on the first iteration
        if (cntr != 0) {
            if (curInliers->size > bestInliers->size) {
                // clean up the old
                VecList_destroy(bestInliers);
                Line_destroy(bestL);
                
                bestL = curL;
                bestInliers = curInliers;
            }
        } else {
            bestL = curL;
            bestInliers = curInliers;
        }
        cntr++;
    }
    
    // after many iterations are sure to have the best Line
    Line_addMany(bestL,bestInliers);
    return bestL;
}

// TODO grabs a random line between points in the vlist
Line * Ransac_randomLine(VecList *vlist) {
    if (vlist->size <= 1) {
        printf("Warning: need atleast 2 points to fit a line: Ransac_randomLine\n");
        exit(1);
    }
    
    Vec *p1 = vlist->list[rand() % vlist->size];
    
    Vec *p2;
    do {
        p2 = vlist->list[rand() % vlist->size];
    } while (Vec_equals(p1,p2));
    
    return Line_2Point(p1,p2);
}

// TODO computes an inlier set for the given line and return
VecList * Ransac_inlierSet(VecList *vlist, Line *l) {
    
    VecList *inliers = VecList_create();
    
    VecList *endpoints = l->points;
    
    float Pdist,Cdist; // perpendicular distance to line and parallel distance from the centroid
    int i,j;
    int flag = TRUE;
    for (i = 0; i < vlist->size; i++) {
        Vec *p = vlist->list[i];
        Pdist = Line_distance(l,p);
        if (Pdist < PTHRESHOLD) {
            flag = FALSE;
            
            // check to make sure isnt' an endpoint already
            for (j = 0; j < endpoints->size; j++) {
                // can just compare pointers hopefully
                if (p == endpoints->list[j]) {
                    flag = TRUE;
                }
            }
            
            // check to make sure not too far from the centroid
            Cdist = Line_distanceP(l,p);
            if (Cdist > CTHRESHOLD) {
                flag = TRUE;
            }
            
            // add to inlier list
            if (!flag) {
                VecList_add(inliers,p);
            }
        }
    }
    return inliers;
    
}
